
#include <agv_peripheral_controller/roller/barrier_control.h>
namespace peripheral_roller
{
    ///////////////////////////////CBarrierControl 实现///////////////////////////////////////////////
    // 默认构造函数
    CBarrierControl::CBarrierControl()
    {
        m_init = false;
    }

    // 析构函数
    CBarrierControl::~CBarrierControl()
    {
        m_subIoNew.shutdown();
    }

    // 是否初始化完成
    bool CBarrierControl::initFinish()
    {
        if (!m_init)
        {
            DEBUG_ERROR_OUT("BarrierControl init failed");
        }
        return m_init;
    }

    // 初始化函数
    bool CBarrierControl::init()
    {

        // 订阅输入信号话题/readInput
        m_subIoNew = m_priNh.subscribe("/readInputNew", 5, &CBarrierControl::readInputNewCallBack, this);
        m_outClient = m_priNh.serviceClient<agv_srvs::srv::OutputsNew>("/outputNew");

        // 初始化io key-名字
        agv_msgs::msg::PairTypeInt io;
        io.value = 0;
        ///////////////////////输入IO/////////////////////////////
        // 前左升降-限位开关
        io.key = UP_LIMIT_FRONT_LEFT;
        m_frontLeftLift.upLimit.Update(io);
        io.key = DOWN_LIMIT_FRONT_LEFT;
        m_frontLeftLift.downLimit.Update(io);
        // 前左升降-故障检测
        io.key = FAULT_FRONT_LEFT;
        m_frontLeftLift.errorIn.Update(io);

        // 前右升降-限位开关
        io.key = UP_LIMIT_FRONT_RIGHT;
        m_frontRightLift.upLimit.Update(io);
        io.key = DOWN_LIMIT_FRONT_RIGHT;
        m_frontRightLift.downLimit.Update(io);
        // 前右升降-故障检测
        io.key = FAULT_FRONT_RIGHT;
        m_frontRightLift.errorIn.Update(io);

        ///////////////////////输出IO/////////////////////////////
        //  前左升降控制
        //  如果数量为2 默认第一个升，第二个io为降
        io.key = CONTROL_UP_FRONT_LEFT;
        m_frontLeftLift.ctrl.push_back(io);
        io.key = CONTROL_DOWN_FRONT_LEFT;
        m_frontLeftLift.ctrl.push_back(io);
        //  前左升降清错io
        io.key = CLEAR_FAULT_FRONT_LEFT;
        m_frontLeftLift.clearFaultOut.push_back(io);

        //  前右升降控制
        //  如果数量为2 默认第一个升，第二个io为降
        io.key = CONTROL_UP_FRONT_RIGHT;
        m_frontRightLift.ctrl.push_back(io);
        io.key = CONTROL_DOWN_FRONT_RIGHT;
        m_frontRightLift.ctrl.push_back(io);
        //  前右升降清错io
        io.key = CLEAR_FAULT_FRONT_LEFT;
        m_frontRightLift.clearFaultOut.push_back(io);

        // 手动旋钮开关
        io.key = SWITCH_UP_BARRIER;
        m_upSwitch.Update(io);
        io.key = SWITCH_DOWN_BARRIER;
        m_downSwitch.Update(io);

        // 初始IO控制输出客户端
        m_frontLeftLift.init(m_outClient);
        m_frontRightLift.init(m_outClient);

        m_init = true;
        return true;
    }

    // 举升上升
    bool CBarrierControl::up()
    {
        bool finish = false;
        if (!initFinish())
        {
            return finish;
        }

        // 控制下降输出
        bool ret = true;
        ret &= m_frontLeftLift.up();
        ret &= m_frontRightLift.up();
        DEBUG_WARN_OUT("BarrierControl:up :"
                       << ",ret=" << ret);

        finish = ret;

        return finish;
    }

    // 举升下降
    bool CBarrierControl::down()
    {
        bool finish = false;
        if (!initFinish())
        {
            return finish;
        }

        // 控制下降输出
        bool ret = true;
        ret &= m_frontLeftLift.down();
        ret &= m_frontRightLift.down();
        DEBUG_WARN_OUT("BarrierControl:down :"
                       << ",ret=" << ret);

        finish = ret;

        return finish;
    }

    // 所有举升停止
    bool CBarrierControl::stop()
    {
        bool finish = false;
        if (!initFinish())
        {
            return finish;
        }

        bool ret = true;
        ret &= m_frontLeftLift.stop();
        ret &= m_frontRightLift.stop();
        DEBUG_WARN_OUT("BarrierControl:stop :"
                       << ",ret=" << ret);
        finish = ret;

        if (finish)
        {
            DEBUG_WARN_OUT("stop lift ok");
        }
        return finish;
    }

    // 触发了错误
    bool CBarrierControl::fault()
    {
        return m_frontLeftLift.hasError() || m_frontRightLift.hasError();
    }

    // 具体的报错信息
    std::string CBarrierControl::faultMsg()
    {
        bool e1 = m_frontLeftLift.hasError();
        bool e2 = m_frontRightLift.hasError();

        std::string msg;
        msg += e1 ? "left barrier  motor error; " : "";
        msg += e2 ? "right barrier  motor error" : "";

        return msg;
    }

    // 清除错误
    bool CBarrierControl::clearFault()
    {
        bool ret1 = m_frontLeftLift.clearErrorOnce();
        bool ret2 = m_frontRightLift.clearErrorOnce();
        return ret1 || ret2;
    }

    // 手动旋钮开关
    SwitchState CBarrierControl::switchBtn()
    {
        if (!initFinish())
        {
            return SwitchState::STOP;
        }
        bool up = inputIo(m_upSwitch);
        bool down = inputIo(m_downSwitch);
        if (up && down)
        {
            DEBUG_ERROR_OUT("BarrierControl switchBtn error");
            return SwitchState::STOP;
        }

        return up ? SwitchState::UP : (down ? SwitchState::DOWN : SwitchState::STOP);
    }

    bool CBarrierControl::upTarget()
    {
        bool target = false;
        if (!initFinish())
        {
            return target;
        }

        target = m_frontLeftLift.upTarget() && m_frontRightLift.upTarget();
        return target;
    }

    bool CBarrierControl::downTarget()
    {
        bool target = false;
        if (!initFinish())
        {
            return target;
        }

        target = m_frontLeftLift.downTarget() && m_frontRightLift.downTarget();
        return target;
    }

    // 货物装填
    bool CBarrierControl::loadStatus()
    {
        bool load = false;
        if (!initFinish())
        {
            return load;
        }
        // 液压举升不在最低处，防止撞工位，使用高度作为有无货物的其中一种条件
        load = m_frontLeftLift.loadStatus() || m_frontRightLift.loadStatus();
        return load;
    }

    // 新更新输入IO对象
    void CBarrierControl::updateInutIoNew(const agv_msgs::msg::ReadInPutsNew &msg, INPUT_IO_TYPE &inputIoObj)
    {
        auto inputIo = inputIoObj.Get();
        std::vector<agv_msgs::msg::PairTypeInt> inputs_msg = msg.array;
        for (auto &data : inputs_msg)
        {
            if (data.key == inputIo.key)
            {
                inputIoObj.Update(data);
                // DEBUG_OUT("updateInutIoNew:inputIo.key=" << inputIo.key << ",data.value=" << data.value);
                break;
            }
        }
    }

    // 获取IO输入信号回调函数 io topic话题频率30hz
    void CBarrierControl::readInputNewCallBack(const agv_msgs::msg::ReadInPutsNew &msg)
    {
        // 获取举升上下限位IO信号 并更新数据的时间戳
        updateInutIoNew(msg, m_frontLeftLift.upLimit);
        updateInutIoNew(msg, m_frontLeftLift.downLimit);
        updateInutIoNew(msg, m_frontLeftLift.errorIn);

        updateInutIoNew(msg, m_frontRightLift.upLimit);
        updateInutIoNew(msg, m_frontRightLift.downLimit);
        updateInutIoNew(msg, m_frontRightLift.errorIn);

        // 获取手动旋钮开关的状态
        updateInutIoNew(msg, m_upSwitch);
        updateInutIoNew(msg, m_downSwitch);
    }

}; // end namespace
